
#include "cmd_func.h"
#include "stdlib.h"
#include "oslib_uart.h"
#include "FreeRTOS.h"
#include "task.h"

#include "chassis.h"
#include "light.h"
#include "ultrasonic.h"
#include "servo.h"
#include "action.h"
#include "switch.h"
#include "dispatch.h"

#ifdef OSLIB_CAN_MODULE_ENABLED
    #include "oslib_can.h"
#endif

#if defined(OSLIB_UART_MODULE_ENABLED) && USE_OSLIB_UART_CLI
/* 在这里添加命令回调函数的定义或外部声明 */
static void Command_Hello(int argc, char *argv[])
{
    uprintf("Hello SimpleLib!\r\n");
}

static void Command_Echo(int argc, char *argv[])
{
    for (int i = 1; i < argc; i++)
        uprintf("Echo: %s\r\n", argv[i]);
}

static void Command_ChassisBreak(int argc, char *argv[])
{
    STOP();
    uprintf("emergency break\r\n");
}

static void Command_ChassisFB(int argc, char *argv[])
{
    if ( argc == 2)
    {
        int8_t speed = atoi(argv[1]);
        Move_FB(speed);
        uprintf("chassis set %d forward\r\n", speed);
    }
    else
    {
        uprintf("COMMAND ERROER!\r\n");
    }
}

static void Command_ChassisCW(int argc, char *argv[])
{
    if ( argc == 2)
    {
        int8_t speed = atoi(argv[1]);
        Move_CW(speed);
        uprintf("chassis set %d clockwise\r\n", speed);
    }
    else
    {
        uprintf("COMMAND ERROER!\r\n");
    }
}

static void Command_ChassisLF(int argc, char *argv[])
{
    if ( argc == 2)
    {
        int8_t speed = atoi(argv[1]);
        Move_LF(speed);
        uprintf("chassis set %d move left\r\n", speed);
    }
    else
    {
        uprintf("COMMAND ERROER!\r\n");
    }
}

static void Command_ChassisFL(int argc, char *argv[])
{
    if ( argc == 3)
    {
        int8_t radius = atoi(argv[1]);
        int8_t vel = atoi(argv[2]);
        Turn_FL(radius, vel);
        uprintf("chassis set bending radius %d; velocity %d\r\n", radius, vel);
    }
    else
    {
        uprintf("COMMAND ERROER!\r\n");
    }
}

static void Command_Stop(int argc, char *argv[])
{
    dispatch_stop();
}

static void Command_Start(int argc, char *argv[])
{
    dispatch_start();
}

static void Command_Light(int argc, char *argv[])
{
    if (argc == 5) 
    {
        uint8_t which = atoi(argv[1]);
        uint8_t R = atoi(argv[2]);
        uint8_t G = atoi(argv[3]);
        uint8_t B = atoi(argv[4]);
        light_set_color(which, R, G, B);
        uprintf("Light: which=%hhd, RGB=%02hhx%02hhx%02hhx\n", which, R, G, B);
    }
    else
    {
        uprintf("Usage: light <which> <R> <G> <B>\n");
    }
}

static void Command_Ultra(int argc, char *argv[])
{
    if (argc == 2) 
    {
        uint8_t id = atoi(argv[1]);
        uprintf("Reading Ultra%hhd\n", id);
        uprintf("%.2f\n", ultra_read(id));
    }
    else 
    {
        uprintf("Usage: ultra <id>\n");
    }
}

static void Command_Servo(int argc, char *argv[])
{
    if (argc == 3) 
    {
        int id = atoi(argv[1]);
        int angle = atoi(argv[2]);
        switch (id) 
        {
            case 1:
                servo_set_angle(&htim4, TIM_CHANNEL_4, angle);
                uprintf("pwm1 angle: %d\n", servo_get_angle(&htim4, TIM_CHANNEL_4));
                break;
            case 2:
                servo_set_angle(&htim4, TIM_CHANNEL_3, angle);
                uprintf("pwm2 angle: %d\n", servo_get_angle(&htim4, TIM_CHANNEL_3));
                break;
            case 3:
                servo_set_angle(&htim4, TIM_CHANNEL_2, angle);
                uprintf("pwm3 angle: %d\n", servo_get_angle(&htim4, TIM_CHANNEL_2));
                break;
            case 4:
                servo_set_angle(&htim4, TIM_CHANNEL_1, angle);
                uprintf("pwm4 angle: %d\n", servo_get_angle(&htim4, TIM_CHANNEL_1));
                break;
            case 5:
                servo_set_angle(&htim8, TIM_CHANNEL_4, angle);
                uprintf("pwm5 angle: %d\n", servo_get_angle(&htim8, TIM_CHANNEL_4));
                break;
            default:
                uprintf("Error pwm number\n");
                break;
        }
    }
    else 
    {
        uprintf("Usage: servo <id> <angle>\n");
    }
}

static void Command_Door(int argc, char *argv[]) 
{
    if (argc == 2) 
    {
        uint8_t mode = atoi(argv[1]);
        action_door(mode);
    }
    else
    {
        uprintf("Usage: door <mode> (mode=0/1)\n");
    }
}

static void Command_Arm(int argc, char *argv[])
{
    if (argc == 2) 
    {
        uint8_t mode = atoi(argv[1]);
        action_arm(mode);
    }
    else
    {
        uprintf("Usage: arm <mode> (mode=0/1)\n");
    }
}

static void Command_Switch(int argc, char *argv[])
{
    uprintf("S1=%hhd S2=%hhd S3=%hhd S4=%hhd\n", \
        switch_read(1), switch_read(2), switch_read(3), switch_read(4));
}

#ifdef OSLIB_CAN_MODULE_ENABLED
static void Command_CanSend(int argc, char *argv[])
{
    if (argc >= 3) {
        can_msg msg;
        msg.in[0] = 0x40005850;
        msg.in[1] = 0x00000000;
        uint32_t id = strtol(argv[2], NULL, 16);
        if (argv[1][0] == 's') {
            can_send_msg(id, &msg);
            uprintf("CanSend:std[%x]\r\n", id);
        } else if (argv[1][0] == 'e') {
            can_ext_send_msg(id, &msg);
            uprintf("CanSend:ext[%x]\r\n", id);
        } else
            uprintf("Param Error!\r\n");
    } else {
        uprintf("Param Error!\r\n");
    }
}
#endif

#if ((configUSE_TRACE_FACILITY == 1) && (configUSE_STATS_FORMATTING_FUNCTIONS > 0) && (configSUPPORT_DYNAMIC_ALLOCATION == 1))
static char tasklist_buffer[256];
static void Command_Task(int argc, char *argv[])
{
    uprintf("Free Heap: %dB\r\n", xPortGetFreeHeapSize());
    uprintf("Name          \tState\tPrio\tStack\tRank\r\n");
    vTaskList(tasklist_buffer);
    uprintf(tasklist_buffer);
    // uprintf("Name          \tCount\tUtilization\r\n");
    // vTaskGetRunTimeStats(tasklist_buffer);
    // uprintf(tasklist_buffer);
}
#endif

/* 在这里完成命令到回调函数的映射 */
void Command_Init()
{
    OSLIB_UartCmd_AddCommand("hello", "\"Hello SimpleLib!\"", Command_Hello);   /* 输出 "Hello SimpleLib!" */   // 测试串口输出和命令
    OSLIB_UartCmd_AddCommand("echo", "Echo message", Command_Echo);             /* 回显参数的内容 */              // 测试命令参数解析
    OSLIB_UartCmd_AddCommand("chassis_break", "Chassis break", Command_ChassisBreak);
    OSLIB_UartCmd_AddCommand("chassis_fb", "Chassis set straight speed (int8_t)<param1>", Command_ChassisFB);
    OSLIB_UartCmd_AddCommand("chassis_cw", "Chassis set clockwise speed (int8_t)<param1>", Command_ChassisCW);
    OSLIB_UartCmd_AddCommand("chassis_lf", "Chassis set left move speed (int8_t)<param>", Command_ChassisLF);
    OSLIB_UartCmd_AddCommand("chassis_fl", "Chassis set bending radius (int8_t)<param1>; velocity (int8_t)<param2>", Command_ChassisFL);
    OSLIB_UartCmd_AddCommand("light", "Light set rgb <which> <R> <G> <B>", Command_Light);
    OSLIB_UartCmd_AddCommand("ultra", "Ultrasonic read <id>", Command_Ultra);
    OSLIB_UartCmd_AddCommand("servo", "Serv set angle <id> <angle>", Command_Servo);
    OSLIB_UartCmd_AddCommand("door", "Door set open/close <mode> (0=close, 1=open)", Command_Door);
    OSLIB_UartCmd_AddCommand("arm", "Arm set open/close <mode> (0=close, 1=open)", Command_Arm);
    OSLIB_UartCmd_AddCommand("switch", "Read switch", Command_Switch);
    OSLIB_UartCmd_AddCommand("dstart", "Dispatch Start", Command_Start);
    OSLIB_UartCmd_AddCommand("dstop", "Dispatch Stop", Command_Stop);
#ifdef OSLIB_CAN_MODULE_ENABLED
    OSLIB_UartCmd_AddCommand("cansend", "CAN send message", Command_CanSend);   /* 发送CAN报文, 内容为两个整数 */  // 测试CAN发送
#endif
#if ((configUSE_TRACE_FACILITY == 1) && (configUSE_STATS_FORMATTING_FUNCTIONS > 0) && (configSUPPORT_DYNAMIC_ALLOCATION == 1))
    OSLIB_UartCmd_AddCommand("tasks", "Show task list", Command_Task);
#endif
}

#endif